%% 目标固定在一点，一个测向站位置给定，优化另两个测向站的位置
clc
clear
close all

T =  [0 50];
S1 = [0 40]; % 可调
fun = @(S23)FixedGdop(T,S1,S23);

A = [];
b = [];
Aeq = [];
beq = [];
lb = [-100 -100 -100 -100];
ub = [100 40 100 40]; % 可调
S0 = [lb(1)+(ub(1)-lb(1))/3 lb(2)+(ub(2)-lb(2))/3 lb(1)+(ub(1)-lb(1))*2/3 lb(2)+(ub(2)-lb(2))/3];
% [S_min,gdop_min] = fmincon(fun,S0,A,b,Aeq,beq,lb,ub);
[S_min, gdop_min] = ga(fun,4,A,b,Aeq,beq,lb,ub);
gdop0 = FixedGdop(T,S1,S0);

figure
plot(T(1),T(2),'*','LineWidth',2);hold;
plot([S0(1) S0(3)],[S0(2) S0(4)],'x','LineWidth',2);
plot([S_min(1) S_min(3) S1(1)],[S_min(2) S_min(4) S1(2)],'o','LineWidth',2);
legend('目标','初始点','最优点','FontSize',14)
xlabel('X/km');ylabel('Y/km');
rectangle('Position',[lb(1) lb(2) ub(1)-lb(1) ub(2)-lb(2)],...
'EdgeColor','k', 'LineWidth',2);
legend('目标','初始点','最优点','FontSize',12)
axis([-100 100 -100 100])
% txt = ['GDOP_m_i_n = ', num2str(fval)];
% text(T(1),T(2),txt);